package com.d2rv.vrpn;

import com.d2rv.experience.SceneManager;
import com.d2rv.settings.Settings;
import processing.core.PVector;
import vrpn.ButtonRemote;
import vrpn.TrackerRemote;

/**
 * VRPNClient for kinect Listen to updates buttons and trackers positions
 *
 * @author Darrieutort A., Dupouy A., Rossignol L., Van Poucke A.
 */
public class VRPNClient {

    private SceneManager scenemanager;
    private ButtonRemote button;
    private TrackerRemote tracker;
    private VRPNButtonListener buttonlistener;
    private VRPNTrackerListener trackerlistener;
    private LowFilter lowfilter;
    private boolean[] states;
    private PVector headposition;
    private PVector leftelbow;
    private PVector rightelbow;
    private PVector leftshoudler;
    private PVector rightshoudler;
    private PVector lefthip;
    private PVector righthip;

    /**
     * Default constructor
     *
     * @param ip IP to listen (VRPN server)
     * @param scenemanager The scene manager uses to move the user
     */
    public VRPNClient(String ip, SceneManager scenemanager) {
        String buttonname = "Button0@" + ip;
        String trackername = "Tracker0@" + ip;
        try {
            this.button = new ButtonRemote(buttonname, null, null, null, null);
            this.tracker = new TrackerRemote(trackername, null, null, null, null);
        } catch (InstantiationException e) {
            System.err.println("Unexpected error while creating the vrpn tracker/button tracking : " + e.getMessage());
        }
        this.states = new boolean[10];
        for (int i = 0; i < this.states.length; i++) {
            this.states[i] = false;
        }
        //Button listener : listen to the buttons update
        this.buttonlistener = new VRPNButtonListener(this.states);
        this.button.addButtonChangeListener(this.buttonlistener);
        this.headposition = new PVector(0, 0, 0);
        this.leftelbow = new PVector(0, 0, 0);
        this.rightelbow = new PVector(0, 0, 0);
        this.leftshoudler = new PVector(0, 0, 0);
        this.rightshoudler = new PVector(0, 0, 0);
        this.lefthip = new PVector(0, 0, 0);
        this.righthip = new PVector(0, 0, 0);
        //Tracker listener : listen to the tracker positions
        this.trackerlistener = new VRPNTrackerListener(this.headposition, this.leftelbow, this.rightelbow, this.leftshoudler, this.rightshoudler, this.lefthip, this.righthip);
        this.tracker.addPositionChangeListener(this.trackerlistener);
        try {
            this.lowfilter = new LowFilter(30, 1.0f, 1.0f, 1.0f);
        } catch (Exception ex) {
            System.err.println("Error while creating LowFilter for VRPN : " + ex.getMessage());
        }
        this.scenemanager = scenemanager;
    }

    /**
     * Update function of the vrpn client : updates head position (head
     * tracking) and allows the user to move and rotate correspondance FAAST : 0
     * : AvancerD 1 : AvancerG 2 : TournerD 3 : TournerG 4 : Demi-tour 6 :
     * AvancerD + Tourner D 7 : AvancerG + Tourner D 8 : AvancerD + Tourner G 9
     * : AvancerG + Tourner G
     * @param headtracking to test head-tracking activation
     */
    public void update(boolean headtracking) {
        Settings s = Settings.getInstance();
        float mx = s.movespeed().x;
        float mz = s.movespeed().z;
        float ry = s.rotatespeed().y;

        //gesture mode set to move with foots
        if (s.gesturemode() == 2) {
            if (this.states[6]) {
                this.scenemanager.moveUser(new PVector(mx, 0, mz));
                this.scenemanager.rotateUser((float) (Math.toRadians(-ry)));
            }
            if (this.states[9]) {
                this.scenemanager.moveUser(new PVector(mx, 0, mz));
                this.scenemanager.rotateUser((float) (Math.toRadians(ry)));
            }
        } else if (headtracking) {
            //update head position
            PVector tmp = new PVector(-10 * this.headposition.z - s.distancekinecttv(), 10 * this.headposition.y - s.distancekinectfloor(), 10 * this.headposition.x - s.offseteyes());
            this.scenemanager.moveUserHead(tmp);
        }

        if (this.states[4]) {
            this.scenemanager.rotateUserTurnBack();
            this.states[4] = false;
        }
        if (this.states[7]) {
            this.scenemanager.rotateUser((float) (Math.toRadians(-ry)));
            this.scenemanager.moveUser(new PVector(mx, 0, mz));
        }
        if (this.states[8]) {
            this.scenemanager.rotateUser((float) (Math.toRadians(ry)));
            this.scenemanager.moveUser(new PVector(mx, 0, mz));
        }
        if (!this.states[4] && !this.states[5] && !this.states[6] && !this.states[7] && !this.states[8] && !this.states[9]) {
            if (this.states[0]) {
                this.scenemanager.moveUser(new PVector(mx, 0, mz));
            }
            if (this.states[1]) {
                this.scenemanager.moveUser(new PVector(mx, 0, mz));
            }
            if (this.states[2]) {
                this.scenemanager.rotateUser((float) (-(Math.toRadians(computeAngleRight()))));
            }
            if (this.states[3]) {
                this.scenemanager.rotateUser((float) (Math.toRadians(computeAngleLeft())));
            }
        }
    }

    /**
     * Compute the angle between (rightshoulder -> rightelbow) and
     * (rightshoudler -> righthip) Increases the precision (speed of the
     * movement) when the user turns to the right
     *
     * @return the angle between (rightshoulder -> rightelbow) and
     * (rightshoudler -> righthip)
     */
    public double computeAngleRight() {
        PVector a = new PVector(this.rightelbow.x - this.rightshoudler.x, this.rightelbow.y - this.rightshoudler.y, this.rightelbow.z - this.rightshoudler.z);
        PVector b = new PVector(this.righthip.x - this.rightshoudler.x, this.righthip.y - this.rightshoudler.y, this.righthip.z - this.rightshoudler.z);
        return PVector.angleBetween(a, b) * 0.03f;
    }

    /**
     * Compute the angle between (leftshoulder -> leftelbow) and (leftshoudler
     * -> lefthip) Increases the precision (speed of the movement) when the user
     * turns to the left
     *
     * @return the angle between (leftshoulder -> leftelbow) and (leftshoudler
     * -> lefthip)
     */
    public double computeAngleLeft() {
        PVector a = new PVector(this.leftelbow.x - this.leftshoudler.x, this.leftelbow.y - this.leftshoudler.y, this.leftelbow.z - this.leftshoudler.z);
        PVector b = new PVector(this.lefthip.x - this.leftshoudler.x, this.lefthip.y - this.leftshoudler.y, this.lefthip.z - this.leftshoudler.z);
        return PVector.angleBetween(a, b) * 0.03f;
    }
}
